package com.rockwell.robot2015.model.states.running.sequence;

import java.security.SecureRandom;

import org.opencv.core.Rect;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.detectors.TreasureDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public class FindingMazeSequence extends RobotRunningSequence {
	public static final int TURN_DEGREES = 15;
	public static final int SCAN_RANGE = 45;
	public static final int BIG_STEP = 50;
	public static final int SMALL_STEP = 10;
	
	public static final int MAX_TREASURE_PICKED = 1;
	
	private final static int BLOCK_ANGLE_THRESHOLD = 10;

	private final static int AWAY_BLOCK_ANGLE = 20;

	static int m_turnedDegrees = 0;
	static int m_turnDirection = 1;
	static boolean m_turnedBack;
	
	static SecureRandom m_random = new SecureRandom();
	
	public enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().log("Start finding maze", 2);

				m_turnedDegrees = 0;
				m_turnedBack = false;
				
				machine.robot().getSensor().setInMaze(false);

				machine.changeState(Detect);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Detect {
			@Override
			public void enter(IStateMachine machine) {
				//machine.robot().log("Enter FindingMaze.Detect" , 2);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				//machine.robot().log("Move FindingMaze.Detect" , 2);

				IDetector mazeDetector = RobotRunningSequence.getMazeDetector(machine.robot());
				IDetector treasureDetector = robot.getTreasureDetector();
				
				/*
				if (m_detectTreasure && robot.treasurePicked() < MAX_TREASURE_PICKED) {
					if (treasureDetector.isPresent()) {
						if (mazeDetector != null) {
							// Found both treasure and maze
							
							Rect maze = mazeDetector.getBoundingRect();
							Rect treasure = treasureDetector.getBoundingRect();
							
							if (containsRect(maze, treasure)) {
								machine.changeState(TreasureFound);
							}
							else {
								machine.changeState(MazeFound);
							}
						}
						else {
							machine.changeState(TreasureFound);
						}
					}
					else {
						tryDetectMaze(machine);
					}
				}
				else {
					tryDetectMaze(machine);
				}
				*/
				tryDetectMaze(machine);
			}
			
			@Override
			public void exit(IStateMachine machine) {
				//machine.robot().log("Exit FindingMaze.Detect" , 2);
			}
			
		},
		Scan {

			@Override
			public void enter(IStateMachine machine) {
				//machine.robot().log("Enter FindingMaze.Turn" , 2);
			}

			@Override
			public void move(IStateMachine machine) {
				//machine.robot().log("Move FindingMaze.Turn" , 2);

				// Send Turn command
				Robot robot = machine.robot();
				IRobotSensors sensor = robot.getSensor();
				
				if (!sensor.isLeftFrontTooClose() && !sensor.isLeftBackTooClose()) {
					robot.sendCommand(RobotAction.TurnDegree, TURN_DEGREES);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				}
				else {
					robot.log("Too close to left wall", 4);
				}
				
				m_turnedDegrees += TURN_DEGREES;
				
				machine.changeState(Detect);
			}

			@Override
			public void exit(IStateMachine machine) {
				//machine.robot().log("Exit FindingMaze.Turn" , 2);
			}
			
		},
		Move {
			@Override
			public void enter(IStateMachine machine) {
				m_turnedDegrees = 0;
				m_turnedBack = false;
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				TreasureDetector treasureDetector = (TreasureDetector)robot.getTreasureDetector();
				
				if (treasureDetector.isPresent()) {
					int distance = (int)treasureDetector.getDistanceInPercentage();
					int degree = RobotRunningSequence.getOrientation(robot, treasureDetector);

					String log = String.format("Treasure found, distance: %d, angle: %d",
							distance, degree);
					
					robot.log(log, 4);
					
					//if (distance < BLOCK_INDEX_DISTANCE) {
						
						if (Math.abs(degree) < BLOCK_ANGLE_THRESHOLD) {
							if (degree > 0) {
								degree = degree - AWAY_BLOCK_ANGLE;
							}
							else {
								degree = AWAY_BLOCK_ANGLE + degree;
							}
							
							robot.turn(degree);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
					//}
				}
				
				robot.sendCommand(RobotAction.MoveWithSensor, BIG_STEP);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				IRobotSensors sensor = robot.getSensor();
				
				if (sensor.detectFrontWall()) {
					/*
					robot.turnBack();
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					*/
					
					robot.sendCommand(RobotAction.MoveToTreasure, -SMALL_STEP);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					
					if (m_random.nextBoolean()) {
						if (!sensor.detectLeftWall()) {
							robot.sendCommand(RobotAction.TurnLeft);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
						else if (!sensor.detectRightWall()) {
							robot.sendCommand(RobotAction.TurnRight);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
					}
					else {
						if (!sensor.detectRightWall()) {
							robot.sendCommand(RobotAction.TurnRight);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
						else if (!sensor.detectLeftWall()) {
							robot.sendCommand(RobotAction.TurnLeft);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
					}
				}
				
				machine.changeState(Detect);
			}

			@Override
			public void exit(IStateMachine machine) {
				//machine.robot().log("Exit FindingMaze.Move" , 2);
			}
			
		},
		MazeFound {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Maze found" , 2);
			}

			@Override
			public void move(IStateMachine machine) {
				//machine.robot().log("Move FindingMaze.MazeFound" , 2);

			}

			@Override
			public void exit(IStateMachine machine) {
				//machine.robot().log("Exit FindingMaze.MazeFound" , 2);

			}
			
		},
		TreasureFound {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Treasure found" , 2);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		};
		
		static FindingMazeSequence m_sequence;
		
		public static void setSequence(FindingMazeSequence sequence) {
			m_sequence = sequence;
		}
	}
	
	public static boolean containsRect(Rect outer, Rect inner) {
		if (inner.x < outer.x) return false;
		if (inner.y < outer.y) return false;
		
		if (inner.x + inner.width > outer.x + outer.width) return false;
		if (inner.y + inner.height > outer.y + outer.height) return false;
		
		return true;
	}
	
	public static void tryDetectMaze(IStateMachine machine) {
		Robot robot = machine.robot();
		
		IDetector mazeDetector = RobotRunningSequence.getMazeDetector(robot);

		if (mazeDetector != null) {
			machine.changeState(State.MazeFound);
		}
		else if (m_turnedDegrees < SCAN_RANGE) {
			machine.changeState(State.Scan);
		}
		else {
			if (m_turnedBack) {
				machine.changeState(State.Move);
			}
			else {
				robot.sendCommand(RobotAction.TurnDegree, 
						-(SCAN_RANGE + m_turnedDegrees) * m_turnDirection);
				
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				m_turnedBack = true;
				m_turnedDegrees = 0;
				
				machine.changeState(State.Scan);
			}
		}
	}
	
	public FindingMazeSequence(RobotRunningStateMachine running) {
		super(running, State.Start);
		
		State.setSequence(this);
		m_currState = State.Start;
	}

	@Override
	public void start() {
		//m_running.robot().log("Finding maze", 2);
		m_turnedDegrees = 0;
		m_turnedBack = false;

		m_currState = State.Start;
		m_nextState = null;
		
		Robot robot = m_running.robot();
		robot.enableTreasureDetector(true);
		//robot.enableObstacleDetector(true);
		robot.enableMaze1Detector(true);
		robot.enableMaze2Detector(true);
		robot.enableMaze3Detector(true);
		robot.enableMazeEntranceGuideDetector(true);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(true);
	}
	
	@Override
	public void reset() {
		super.reset();
		
		resetTurnedDegrees();
	}
	
	@Override
	public boolean succeeded() {
		return m_currState.equals(State.MazeFound) /*|| m_currState.equals(State.TreasureFound)*/;
	}

	public boolean isMazeFound() {
		return m_currState.equals(State.MazeFound);
	}
	
	public boolean isTreasureFound() {
		return m_currState.equals(State.TreasureFound);
	}
	

	@Override
	public boolean failed() {
		// TODO Auto-generated method stub
		return false;
	}
	
	public int turnedDegrees() {
		return m_turnedDegrees;
	}
	
	public void onTurned(int degrees) {
		m_turnedDegrees += degrees;
	}

	public void resetTurnedDegrees() {
		m_turnedDegrees = 0;
	}
}
